#pragma once

#include "registration_interface.hpp"

#include <pcl/registration/ndt.h>
#include <pcl/registration/gicp.h>
#include <fast_gicp/gicp/fast_gicp.hpp>
#include <fast_gicp/gicp/fast_gicp_st.hpp>
#include <fast_gicp/gicp/fast_vgicp.hpp>

#include "opt_ICP_GN.hpp"
#include "icp_svd_registration.hpp"
#include "icp_registration.hpp"
#include "ndt_registration.hpp"
#include "sicp/scip_registration.hpp"
#include "ndt_cpu/ndt_cpu_registration.hpp"
#include "opt_ICP_CERES.hpp"
#include "opt_ICP_G2O.hpp"
#include "opt_ICP_GTSAM.hpp"

#ifdef USE_VGICP_CUDA
#include <fast_gicp/ndt/ndt_cuda.hpp>
#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
#endif

#include <kcp/keypoint.hpp>
#include <kcp/solver.hpp>


struct AlignResult {
    double time;
    double score;
    Eigen::Matrix4f tf;
};

void convert_pcl_point_cloud_to_eigen_matrix(CloudPtr& cloud, Eigen::MatrixX3d& matrix) {
    matrix.setZero(cloud->points.size(), 3);
    for (int idx = 0; idx < cloud->points.size(); ++idx) {
        const auto& point = cloud->points[idx];
        matrix(idx, 0) = point.x;
        matrix(idx, 1) = point.y;
        matrix(idx, 2) = point.z;
    }
}


AlignResult* test_kcp( CloudPtr& cloud_target,  CloudPtr& cloud_source) {
    AlignResult* Res = new AlignResult;

    Eigen::MatrixX3d source;
    Eigen::MatrixX3d target;
    convert_pcl_point_cloud_to_eigen_matrix(cloud_source, source);
    convert_pcl_point_cloud_to_eigen_matrix(cloud_target, target);
    Eigen::MatrixX3d source_corner_points = kcp::keypoint::MultiScaleCurvature(source).get_corner_points();
    Eigen::MatrixX3d target_corner_points = kcp::keypoint::MultiScaleCurvature(target).get_corner_points();

    auto params = kcp::KCP::Params();

    params.k = 2;
    params.verbose = true;
    params.teaser.noise_bound = 0.06;
    auto solver = kcp::KCP(params);

    auto t1 = std::chrono::high_resolution_clock::now();
    solver.solve(source_corner_points, target_corner_points, source_corner_points, target_corner_points);
    auto t2 = std::chrono::high_resolution_clock::now();
    Res->time = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
    std::cout << "time:" << Res->time << "[msec]" << std::endl;
    Res->score = 404.0;
    Res->tf = solver.get_solution().cast<float>();
    return Res;
}

template <typename Registration>
AlignResult* test_pcl(Registration& reg, const CloudPtr& target, const CloudPtr& source) {
    CloudPtr aligned(new CloudType);
    AlignResult* Res = new AlignResult;

    // single run
    auto t1 = std::chrono::high_resolution_clock::now();
    reg.setInputTarget(target);
    reg.setInputSource(source);
    reg.align(*aligned);
    auto t2 = std::chrono::high_resolution_clock::now();
    Res->score = reg.getFitnessScore();
    Res->time = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
    Res->tf = reg.getFinalTransformation();
    std::cout << "time:" << Res->time << "[msec] score:" << Res->score << std::endl;

    return Res;
}

// benchmark for fast_gicp registration methods
template <typename Registration>
AlignResult* test_fast(Registration& reg, const CloudPtr& target, const CloudPtr& source) {
    CloudPtr aligned(new CloudType);
    AlignResult* Res = new AlignResult;

    auto t1 = std::chrono::high_resolution_clock::now();
    // fast_gicp reuses calculated covariances if an input cloud is the same as the previous one
    // to prevent this for benchmarking, force clear source and target clouds
    reg.clearTarget();
    reg.clearSource();
    reg.setInputTarget(target);
    reg.setInputSource(source);
    reg.align(*aligned);
    auto t2 = std::chrono::high_resolution_clock::now();
    Res->score = reg.getFitnessScore();
    Res->time = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
    Res->tf = reg.getFinalTransformation();
    std::cout << "time:" << Res->time << "[msec] score:" << Res->score << std::endl;

    return Res;
}